#include "pid_controller.h"
#include "esp_timer.h"  // 获取时间（微秒）

/**
 * @brief 限制值的范围
 */
static inline float constrain(float value, float min_val, float max_val) {
    if (value < min_val) return min_val;
    else if (value > max_val) return max_val;
    else return value;
}

void PIDController_Init(PIDController* pid, float P, float I, float D, float ramp, float limit) {
    pid->P = P;
    pid->I = I;
    pid->D = D;
    pid->output_ramp = ramp;
    pid->limit = limit;

    // 清除历史记录
    pid->error_prev = 0.0f;
    pid->output_prev = 0.0f;
    pid->integral_prev = 0.0f;
    pid->timestamp_prev = esp_timer_get_time();  // 获取当前时间（微秒）
}

float PIDController_Update(PIDController* pid, float error) {
    int64_t timestamp_now = esp_timer_get_time();  // 当前时间（微秒）
    float Ts = (timestamp_now - pid->timestamp_prev) / 1e6f;  // 计算采样周期（秒）

    if (Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;  // 防止异常采样周期（设为 1ms）

    // --- P项 ---
    pid->p_out = pid->P * error;

    // --- I项：Tustin（梯形）积分 ---
    
    pid->i_out = pid->integral_prev + pid->I * Ts * 0.5f * (error + pid->error_prev);
    pid->i_out = constrain(pid->i_out, -pid->limit, pid->limit);  // 限制积分范围（防止积分饱和）

    // --- D项 ---
    
    pid->d_out = pid->D * (error - pid->error_prev) / Ts;

    // --- PID 总输出 ---
    float output = pid->p_out + pid->i_out + pid->d_out;
    output = constrain(output, -pid->limit, pid->limit);  // 限制总输出范围

    // --- 输出变化斜率限制 ---
    if (pid->output_ramp > 0.0f) {
        float output_rate = (output - pid->output_prev) / Ts;
        if (output_rate > pid->output_ramp)
            output = pid->output_prev + pid->output_ramp * Ts;
        else if (output_rate < -pid->output_ramp)
            output = pid->output_prev - pid->output_ramp * Ts;
    }

    // --- 更新历史值 ---
    pid->end_out = output;
    pid->integral_prev = pid->i_out;
    pid->output_prev = output;
    pid->error_prev = error;
    pid->timestamp_prev = timestamp_now;

    return output;
}
